#include "Simulator2D.h"
#include <iostream>
using namespace std;

void Simulator2D::Init(int xGridNum,int yGridNum){


	m_xGridNum=xGridNum;
	m_yGridNum=yGridNum;

	m_iVSize=(m_yGridNum+1)*m_xGridNum+(m_xGridNum+1)*m_yGridNum;
	m_v=new Vector2[m_iVSize];
	m_interV=new Vector2[m_iVSize];
	
	int xLength=(m_yGridNum+1)*m_xGridNum;
	int yLength=(m_xGridNum+1)*m_yGridNum;

	m_grid=new Grid[m_xGridNum*m_yGridNum];

	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];
			curGrid.vDIndex=0		+iX	  +iY*m_xGridNum;
			curGrid.vRIndex=xLength  +iX+1 +iY*(m_xGridNum+1);  
			curGrid.vUIndex=0		+iX   +(iY+1)*m_xGridNum;
			curGrid.vLIndex=xLength  +iX   +iY*(m_xGridNum+1);

			curGrid.pressure=curGrid.interPressure=0;
			curGrid.divergency=0;
			curGrid.mType=Grid::MTYPE_FLUID;

			m_v[curGrid.vDIndex]=Vector2(0,0);
			m_v[curGrid.vRIndex]=Vector2(0,0);
			m_v[curGrid.vUIndex]=Vector2(0,0);
			m_v[curGrid.vLIndex]=Vector2(0,0);

		}
	}


	for (int iX=0;iX<m_xGridNum;iX++){
		m_grid[(m_yGridNum-1)*m_xGridNum+iX].mType=Grid::MTYPE_AIR;
		m_grid[0*m_xGridNum+iX].mType=Grid::MTYPE_SOLID;
	}

	for (int iY=0;iY<m_yGridNum;iY++){
		m_grid[iY*m_xGridNum+0].mType=Grid::MTYPE_SOLID;
		m_grid[iY*m_xGridNum+m_xGridNum-1].mType=Grid::MTYPE_SOLID;
	}


	int speedBegin=m_xGridNum/3;
	int speedEnd=m_xGridNum*2/3;
	for (int iX=speedBegin;iX<speedEnd;iX++){
		Grid &curGrid=m_grid[16*m_xGridNum+iX];
		//m_v[curGrid.vDIndex]=Vector2(0,15.0f);
	}

	m_density=1.0f;
	m_deltaTime=0.00000000001f;
	m_externForce=Vector2(0,0);
}


void Simulator2D::Advection(float timeSinceLastFrame){
	memcpy(m_interV,m_v,sizeof(Vector2)*m_iVSize);
	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];

			Vector2 ldPos(iX,iY);
			if (iX==0){
				advectVelocity(m_v[curGrid.vLIndex],ldPos+Vector2(0,0.5f));
			}	
			if (iY==0){
				advectVelocity(m_v[curGrid.vDIndex],ldPos+Vector2(0.5f,0));
			}
			advectVelocity(m_v[curGrid.vRIndex],ldPos+Vector2(1.0f,0.5f));
			advectVelocity(m_v[curGrid.vUIndex],ldPos+Vector2(0.5f,1.0f));
		}
	}


	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];
			if (curGrid.mType==Grid::MTYPE_SOLID){
				m_v[curGrid.vLIndex].x=0;
				m_v[curGrid.vRIndex].x=0;
				m_v[curGrid.vDIndex].y=0;
				m_v[curGrid.vUIndex].y=0;
			}	
		}
	}


}

#define SQRT2 1.41421356237
#define ONEOVERSQRT2 0.707106781
#define FIVEMINUSONEOVERSQRT2 0.35355339059

void Simulator2D::advectVelocity(Vector2 &v,const Vector2 &curPos){
	Vector2 samplePos=curPos-v*m_deltaTime;

	if (samplePos.x<0||samplePos.x>=m_xGridNum||
		samplePos.y<0||samplePos.y>=m_yGridNum){
			v=Vector2(0,0);
			return;
	}

	int sampleIX=int(samplePos.x);
	int sampleIY=int(samplePos.y);

	float gridX=samplePos.x-sampleIX;
	float gridY=samplePos.y-sampleIY;
 
// 
// 	float newGridX=(gridX+gridY)*0.5f;
// 	float newGridY=(-gridX+gridY)*0.5f-FIVEMINUSONEOVERSQRT2;
// 
// 	Grid &curGrid=m_grid[sampleIX+sampleIY*m_xGridNum];
// 	Vector2 &vl=m_v[curGrid.vLIndex];
// 	Vector2 &vd=m_v[curGrid.vDIndex];
// 	Vector2 &vr=m_v[curGrid.vRIndex];
// 	Vector2 &vu=m_v[curGrid.vUIndex];
// 	
// 
// 	Vector2 vInterLD=vl*newGridX+(1.0-newGridX)*vd;
// 	Vector2 vInterUR=vu*newGridX+(1.0-newGridX)*vr;
// 
// 	Vector2 vInterLDUR=vInterLD*newGridY+(1.0-newGridY)*vInterUR;
// 
// 	v=vInterLDUR;

	float newGridX=gridX;
	float newGridY=gridY;

	Grid &curGrid=m_grid[sampleIX+sampleIY*m_xGridNum];
	Vector2 &vl=m_interV[curGrid.vLIndex];
	Vector2 &vd=m_interV[curGrid.vDIndex];
	Vector2 &vr=m_interV[curGrid.vRIndex];
	Vector2 &vu=m_interV[curGrid.vUIndex];



	Vector2 vInterLDUR;
	vInterLDUR.x=vl.x*newGridX+(1-newGridX)*vr.x;
	vInterLDUR.y=vd.y*newGridY+(1-newGridY)*vu.y;

	v=vInterLDUR;

	
}

void Simulator2D::BodyForce(float timeSinceLastFrame){
	Vector2 acc=m_externForce/m_density;
	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];
			m_v[curGrid.vDIndex]+=acc;
			m_v[curGrid.vRIndex]+=acc;
			m_v[curGrid.vUIndex]+=acc;
			m_v[curGrid.vLIndex]+=acc;
		}
	}
}

void Simulator2D::Projection(float timeSinceLastFrame){
	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];
			curGrid.pressure=0;
		}
	}

 
	//calc the divergency
	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];
			if (curGrid.mType!=Grid::MTYPE_FLUID){
				continue;
			}
			curGrid.divergency=(m_v[curGrid.vRIndex].x-m_v[curGrid.vLIndex].x)+
							   (m_v[curGrid.vUIndex].y-m_v[curGrid.vDIndex].y);
		}
	}

	//calc the pressure using jacobi interaction
	for (int i=0;i<300;i++){
		jacobiIteraction();
	}

	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];
			if (curGrid.mType==Grid::MTYPE_AIR){
				curGrid.pressure=0;
			}
		}
	}
	

	float tempFactor=m_deltaTime/m_density;


	//solve the u for next iter
	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];
			if (curGrid.mType==Grid::MTYPE_SOLID){
				m_v[curGrid.vLIndex].x=0;
				m_v[curGrid.vRIndex].x=0;
				m_v[curGrid.vDIndex].y=0;
				m_v[curGrid.vUIndex].y=0;
			}	 
			else {
				Vector2 ldPos(iX,iY);
// 				if (iX==0){
// 					m_v[curGrid.vLIndex].x+=(m_grid[iY*m_xGridNum+iX-1].pressure-curGrid.pressure)*tempFactor;
// 				}
// 				if (iY==0){
// 					m_v[curGrid.vDIndex].y+=(m_grid[(iY-1)*m_xGridNum+iX].pressure-curGrid.pressure)*tempFactor;
// 				}

				if (iX+1<m_xGridNum){
					if (m_grid[iY*m_xGridNum+iX+1].mType!=Grid::MTYPE_SOLID){
						m_v[curGrid.vRIndex].x+=(curGrid.pressure-m_grid[iY*m_xGridNum+iX+1].pressure)*tempFactor;
					}
				}
				
				if (iY+1<m_yGridNum){
					if (m_grid[(iY+1)*m_xGridNum+iX].mType!=Grid::MTYPE_SOLID){
						m_v[curGrid.vUIndex].y+=(curGrid.pressure-m_grid[(iY+1)*m_xGridNum+iX].pressure)*tempFactor;
					}
				}

			}
		}
	}

}


void Simulator2D::jacobiIteraction(){
	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];
			if (curGrid.mType!=Grid::MTYPE_FLUID){
				continue;
			}

// 			curGrid.interPressure=0.25f*
// 							(-(m_density/m_deltaTime)*curGrid.divergency
// 							 +getGridPressure(iX+1,iY)+getGridPressure(iX,iY+1)+getGridPressure(iX-1,iY)+getGridPressure(iX,iY-1));
			int coef=4;
			coef-=countSolid(iX,iY);
			if (coef==0){
				curGrid.interPressure=0;
				continue;
			}
			float fCoef=1.0f/coef;
 			curGrid.interPressure=fCoef*
 							(-(m_density/m_deltaTime)*curGrid.divergency
 							 +getGridPressure(iX+1,iY)+getGridPressure(iX,iY+1)+getGridPressure(iX-1,iY)+getGridPressure(iX,iY-1));
		}
	}

	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];
			if (curGrid.mType!=Grid::MTYPE_FLUID){
				continue;
			}

			curGrid.pressure=curGrid.interPressure;		

		}
	}
}

float Simulator2D::getGridPressure(int iX,int iY){
	if (iX<0||iX>=m_xGridNum||
		iY<0||iY>=m_yGridNum){
		cerr<<"error!,calc the boundary grid\n";
	}
	if (m_grid[iY*m_xGridNum+iX].mType!=Grid::MTYPE_FLUID){
		return 0;
	}

	return m_grid[iY*m_xGridNum+iX].pressure;
}
int	Simulator2D::countSolid(int iX,int iY){
	if (iX<0||iX>=m_xGridNum||
		iY<0||iY>=m_yGridNum){
			cerr<<"error!,calc the boundary grid\n";
	}
	Grid &curGrid=m_grid[iY*m_xGridNum+iX];

	Grid *gridPtr[4]={&m_grid[iY*m_xGridNum+iX+1],&m_grid[iY*m_xGridNum+iX-1],&m_grid[(iY+1)*m_xGridNum+iX],&m_grid[(iY-1)*m_xGridNum+iX]};
	int count=0;
	for (int i=0;i<4;i++){
		if(gridPtr[i]->mType==Grid::MTYPE_SOLID){
			count++;
		}
	}
	return count;
}


#define  INFSIZEX 3
#define  INFSIZEY 3
void Simulator2D::AddVelocity(const Vector2 &startPos,const Vector2 &dMove){
	int iCenterX=startPos.x;
	int iCenterY=startPos.y;

	for (int iY=0;iY<INFSIZEY;iY++){
		for (int iX=0;iX<INFSIZEX;iX++){
			int x=iX+iCenterX-INFSIZEX/2;
			int y=iY+iCenterY-INFSIZEY/2;

			if (x>=0&&x<m_xGridNum&&
				y>=0&&y<m_yGridNum){
				Grid &curGrid=m_grid[y*m_xGridNum+x];
				m_v[curGrid.vDIndex]+=dMove;
				m_v[curGrid.vRIndex]+=dMove;
				m_v[curGrid.vUIndex]+=dMove;
				m_v[curGrid.vLIndex]+=dMove;
			}
		}
	}
	
}






void Simulator2D::Release(){
	delete [] m_v;
	delete [] m_grid;
}


void Simulator2D::DrawGrid(){
	glPointSize(3.0f);
	glColor3f(1,1,1);
	glBegin(GL_LINES);
	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			glVertex2i(iX,iY);
			glVertex2i(iX+1,iY);

			glVertex2i(iX+1,iY);
			glVertex2i(iX+1,iY+1);

			glVertex2i(iX+1,iY+1);
			glVertex2i(iX,iY+1);

			glVertex2i(iX,iY+1);
			glVertex2i(iX,iY);

		}
	}
	glEnd();
}

void Simulator2D::__drawPoint(int index){
	int xLength=(m_yGridNum+1)*m_xGridNum;

	float fX;
	float fY;

	if (index<xLength){
		fX=index%m_xGridNum+0.5f;
		fY=index/m_xGridNum;
	}
	else {
		index-=xLength;
		fX=index%(m_xGridNum+1);
		fY=index/(m_xGridNum+1)+0.5f;
	}
	
	glVertex2f(fX,fY);
}

void Simulator2D::DrawVelocitySamplePoints(){
	glColor3f(1,0,0);
	glBegin(GL_POINTS);
	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
// 			__drawPoint(m_grid[iY*m_xGridNum+iX].vDIndex);
// 			__drawPoint(m_grid[iY*m_xGridNum+iX].vRIndex);
// 			__drawPoint(m_grid[iY*m_xGridNum+iX].vUIndex);
 			__drawPoint(m_grid[iY*m_xGridNum+iX].vLIndex);
		}
	}
	glEnd();
}

void Simulator2D::DrawSpeedField(){

	for (int iY=0;iY<m_yGridNum;iY++){
		for (int iX=0;iX<m_xGridNum;iX++){
			Grid &curGrid=m_grid[iY*m_xGridNum+iX];
			Vector2 gridAvgV=0.25f*(m_v[curGrid.vDIndex]+m_v[curGrid.vRIndex]+
									m_v[curGrid.vUIndex]+m_v[curGrid.vLIndex]);
			float mag=gridAvgV.length();
			gridAvgV.normalise();
			glColor3f(1,1,0);
			Vector2 endPos=Vector2(iX+0.5f,iY+0.5f)+(mag/10.0f)*gridAvgV;
			glBegin(GL_LINES);
			glVertex2f(iX+0.5f,iY+0.5f);
			glVertex2f(endPos.x,endPos.y);
			glEnd();
		}
	}
}
